#include "calibrater.h"

#include <mutex>

#include "dwarf_utils.h"
#include "elog.h"
#include "elog_file.h"
#include "iTips.h"
#include "ws_constant.h"
#include "ws_response.h"

using namespace std;
namespace fs = std::filesystem;

HourAngleCoordinates Calibrater::zenithCoord_ = {0.0, 90.0};// 默认天顶为北天极
HorizontalCoordinates Calibrater::lastMechanicalCoord_ = {0.0, 0.0};
HorizontalCoordinates Calibrater::lastMyCoord_ = {0.0, 0.0};

bool Calibrater::b_reverse_motor_yaw_mechanical_direction_ = false;
bool Calibrater::b_reverse_motor_pitch_mechanical_direction_ = false;

CalibraterState Calibrater::calibrater_state_;

std::mutex calibrater_state_mutex;

int Calibrater::start(const std::vector<HorizontalCoordinates>& calibration_path) {
    // 设置曝光增益IRCUT
    cam_tele_.setExp(exposure_);
    cam_tele_.setGain(gain_);
    cam_tele_.setIrCut(ir_state_);

    success_times_ = 0;
    EquatorialCoordinates eqCoord[need_success_times_];
    HourAngleCoordinates haCoord[need_success_times_];

    for(const HorizontalCoordinates& mechanical_coord : calibration_path) {
        motor_yaw_.setParameterInPositionInDegree(mechanical_coord.azi, rotate_speed_, motor_resolution_level_, motor_speed_ramping_);
        motor_yaw_.run();

        motor_pitch_.setParameterInPositionInDegree(mechanical_coord.alt, rotate_speed_, motor_resolution_level_, motor_speed_ramping_);
        motor_pitch_.run();

        // 延时2个曝光时间，等待图像稳定
        sleep(exposure_ * 2);

        int ret = plateSolver_.start(eqCoord[success_times_], haCoord[success_times_]);
        if (ret == 0)
            success_times_++;
        else 
            return -1;
    }

    int ret = calculateMyZenithCoord(haCoord, zenithCoord_);
    if (ret < 0)
        return ret;

    Ephemeris::haCoordToMyCoord(haCoord[2], zenithCoord_, lastMyCoord_);

    motor_yaw_.getAbsolutePositionInDegree(motor_resolution_level_, lastMechanicalCoord_.azi);
    motor_pitch_.getAbsolutePositionInDegree(motor_resolution_level_, lastMechanicalCoord_.alt);

    return 0;
}

int Calibrater::start() {
    b_calibration = true;

    // 设置曝光增益IRCUT
    cam_tele_.setExp(exposure_);
    cam_tele_.setGain(gain_);
    cam_tele_.setIrCut(ir_state_);

    // 电机复位
    int ret = motor_yaw_.resetAndSetParameterInDegree();
    if (ret < 0) {
        if (b_calibration == false) {
            wsCommonResponse(CMD_ASTRO_STOP_CALIBRATION, MODULE_ASTRO, CMD_TYPE_RESPONSE, WS_OK);
            setAndNotifyCalibraterState(STATE_STOPPED);
            setAndNotifyCalibraterState(STATE_IDLE);

            return 0;
        }

        ret = motor_yaw_.resetAndSetParameterInDegree(YAW_RIGHT, 170, YAW_LEFT, STEP_MOTOR_NOW_POSITION);
        if (ret < 0)
            motor_yaw_.setParameterInDegree(170.0, 32.0, STEP_MOTOR_MINISTEP_4, STEP_MOTOR_RAMP_PULSE_0, YAW_LEFT);
        else 
            b_reverse_motor_yaw_mechanical_direction_ = true;
    } else {
        b_reverse_motor_yaw_mechanical_direction_ = false;
    }

    if (b_calibration == false) {
        if (itips_observer_->getErrorCode(CMD_ASTRO_START_CALIBRATION) == -1)
            itips_observer_->backToOldCmdIdAndErrorCode(CMD_ASTRO_START_CALIBRATION);

        wsCommonResponse(CMD_ASTRO_STOP_CALIBRATION, MODULE_ASTRO, CMD_TYPE_RESPONSE, WS_OK);
        setAndNotifyCalibraterState(STATE_STOPPED);
        setAndNotifyCalibraterState(STATE_IDLE);

        return 0;
    }

    motor_yaw_.run();

    if (b_calibration == false) {
        if (itips_observer_->getErrorCode(CMD_ASTRO_START_CALIBRATION) == -1)
            itips_observer_->backToOldCmdIdAndErrorCode(CMD_ASTRO_START_CALIBRATION);

        wsCommonResponse(CMD_ASTRO_STOP_CALIBRATION, MODULE_ASTRO, CMD_TYPE_RESPONSE, WS_OK);
        setAndNotifyCalibraterState(STATE_STOPPED);
        setAndNotifyCalibraterState(STATE_IDLE);

        return 0;
    }

    ret = motor_pitch_.resetAndSetParameterInDegree();
    if (ret < 0) {
        if (b_calibration == false) {
            wsCommonResponse(CMD_ASTRO_STOP_CALIBRATION, MODULE_ASTRO, CMD_TYPE_RESPONSE, WS_OK);
            setAndNotifyCalibraterState(STATE_STOPPED);
            setAndNotifyCalibraterState(STATE_IDLE);

            return 0;
        }
        
        ret = motor_pitch_.resetAndSetParameterInDegree(PITCH_UP, 180, PITCH_DOWN, STEP_MOTOR_NOW_POSITION);
        if (ret < 0)
            motor_pitch_.setParameterInDegree(180.0, 32.0, STEP_MOTOR_MINISTEP_4, STEP_MOTOR_RAMP_PULSE_0, PITCH_DOWN);
        else 
            b_reverse_motor_pitch_mechanical_direction_ = true;
    } else {
        b_reverse_motor_pitch_mechanical_direction_ = false;
    }

    if (b_calibration == false) {
        if (itips_observer_->getErrorCode(CMD_ASTRO_START_CALIBRATION) == -1)
            itips_observer_->backToOldCmdIdAndErrorCode(CMD_ASTRO_START_CALIBRATION);

        wsCommonResponse(CMD_ASTRO_STOP_CALIBRATION, MODULE_ASTRO, CMD_TYPE_RESPONSE, WS_OK);
        setAndNotifyCalibraterState(STATE_STOPPED);
        setAndNotifyCalibraterState(STATE_IDLE);
        
        return 0;
    }
    
    motor_pitch_.run();

    motor_yaw_.waitMotorToStop();
    motor_pitch_.waitMotorToStop();

    if (b_calibration == false) {
        if (itips_observer_->getErrorCode(CMD_ASTRO_START_CALIBRATION) == -1)
            itips_observer_->backToOldCmdIdAndErrorCode(CMD_ASTRO_START_CALIBRATION);

        wsCommonResponse(CMD_ASTRO_STOP_CALIBRATION, MODULE_ASTRO, CMD_TYPE_RESPONSE, WS_OK);
        setAndNotifyCalibraterState(STATE_STOPPED);
        setAndNotifyCalibraterState(STATE_IDLE);

        return 0;
    }

    // 左转small_rotate_angle_
    motor_yaw_.setParameterInDegree(small_rotate_angle_, rotate_speed_, motor_resolution_level_, motor_speed_ramping_, YAW_LEFT);
    motor_yaw_.run();
    motor_yaw_.waitMotorToStop();

    if (b_calibration == false) {
        if (itips_observer_->getErrorCode(CMD_ASTRO_START_CALIBRATION) == -1)
            itips_observer_->backToOldCmdIdAndErrorCode(CMD_ASTRO_START_CALIBRATION);

        wsCommonResponse(CMD_ASTRO_STOP_CALIBRATION, MODULE_ASTRO, CMD_TYPE_RESPONSE, WS_OK);
        setAndNotifyCalibraterState(STATE_STOPPED);
        setAndNotifyCalibraterState(STATE_IDLE);

        return 0;
    }

    // 延时2个曝光时间，等待图像稳定
    sleep(exposure_ * 2);

    success_times_ = 0;
    EquatorialCoordinates eqCoord[need_success_times_];
    HourAngleCoordinates haCoord[need_success_times_];
    double need_rotate_angle = 0;
    double next_rotate_angle = 0;

#if !DEBUG_CALIBRATION
    string fitsFolderString = calibrationFolderPrefix + getTimeStamp();
#else
    string fitsFolderString = "test_calibration";
#endif

    fs::path fitsTmpParentPath = tmpPath / fitsFolderString;
    fs::create_directory(fitsTmpParentPath);
    asNetPlateSolver_.setFitsParentPath(fitsTmpParentPath);

    setAndNotifyCalibraterState(STATE_ASTRO_PLATE_SOLVING, 1);

    if (b_calibration == false) {
        if (itips_observer_->getErrorCode(CMD_ASTRO_START_CALIBRATION) == -1)
            itips_observer_->backToOldCmdIdAndErrorCode(CMD_ASTRO_START_CALIBRATION);

        wsCommonResponse(CMD_ASTRO_STOP_CALIBRATION, MODULE_ASTRO, CMD_TYPE_RESPONSE, WS_OK);
        setAndNotifyCalibraterState(STATE_STOPPED);
        setAndNotifyCalibraterState(STATE_IDLE);

        return 0;
    }

    ret = plateSolver_.start(eqCoord[success_times_], haCoord[success_times_]);
    if (ret == 0) {
        success_times_++;
        next_rotate_angle = small_rotate_angle_;

        rgb_power_.setRgbEffect(RGB_EFFECT_WORKING_STATE);
    } else {
        next_rotate_angle = big_rotate_angle_;
        itips_observer_->updateCmdIdAndErrorCode(CMD_ASTRO_START_CALIBRATION, CODE_ASTRO_PLATE_SOLVING_FAILED);
        wsCommonResponse(CMD_ASTRO_START_CALIBRATION, MODULE_ASTRO, CMD_TYPE_RESPONSE, -CODE_ASTRO_PLATE_SOLVING_FAILED);
    }
    need_rotate_angle = small_rotate_angle_ + next_rotate_angle;

    if (b_calibration == false) {
        if (itips_observer_->getErrorCode(CMD_ASTRO_START_CALIBRATION) == -1)
            itips_observer_->backToOldCmdIdAndErrorCode(CMD_ASTRO_START_CALIBRATION);
            
        wsCommonResponse(CMD_ASTRO_STOP_CALIBRATION, MODULE_ASTRO, CMD_TYPE_RESPONSE, WS_OK);
        setAndNotifyCalibraterState(STATE_STOPPED);
        setAndNotifyCalibraterState(STATE_IDLE);

        return 0;
    }

    // 循环尝试(max_try_times_-1)次
    for(int i = 1; i < max_try_times_; i++) {
        // 判断旋转need_rotate_angle是否会撞限位
        double end_position = 0;
        if(motor_yaw_.getEndPositionInDegree(need_rotate_angle, motor_resolution_level_, YAW_RIGHT, end_position) >= 0) {
            ret = motor_yaw_.checkPositionInDegree(end_position);
            if (ret != 0) {
                // 转存到SD卡
                fs::path fitsFailedParentPath = failedPath / fitsFolderString;

                try {
                    fs::copy(fitsTmpParentPath, fitsFailedParentPath);
                } catch (const std::filesystem::filesystem_error& e) {
                    log_i("fs::copy:%s", e.what());
                }

                itips_observer_->updateCmdIdAndErrorCode(CMD_ASTRO_START_CALIBRATION, CODE_STEP_MOTOR_LIMIT_POSITION_WARNING);
                wsCommonResponse(CMD_ASTRO_START_CALIBRATION, MODULE_ASTRO, CMD_TYPE_RESPONSE, -CODE_STEP_MOTOR_LIMIT_POSITION_WARNING);
                wsCommonResponse(CMD_ASTRO_START_CALIBRATION, MODULE_ASTRO, CMD_TYPE_RESPONSE, -CODE_ASTRO_CALIBRATION_FAILED);
                
                setAndNotifyCalibraterState(STATE_STOPPED);
                setAndNotifyCalibraterState(STATE_IDLE);

                return ret;
            }
        }

        if (b_calibration == false)
            break;

        // 右转next_rotate_angle
        motor_yaw_.setParameterInDegree(next_rotate_angle, rotate_speed_, motor_resolution_level_, motor_speed_ramping_, YAW_RIGHT);
        motor_yaw_.run();
        ret = motor_yaw_.waitMotorToStop();
        if (ret == -CODE_STEP_MOTOR_LIMIT_POSITION_HITTED) {
            itips_observer_->updateCmdIdAndErrorCode(CMD_ASTRO_START_CALIBRATION, CODE_STEP_MOTOR_LIMIT_POSITION_HITTED);
            wsCommonResponse(CMD_ASTRO_START_CALIBRATION, MODULE_ASTRO, CMD_TYPE_RESPONSE, -CODE_STEP_MOTOR_LIMIT_POSITION_HITTED);
            wsCommonResponse(CMD_ASTRO_START_CALIBRATION, MODULE_ASTRO, CMD_TYPE_RESPONSE, -CODE_ASTRO_CALIBRATION_FAILED);
            
            setAndNotifyCalibraterState(STATE_STOPPED);
            setAndNotifyCalibraterState(STATE_IDLE);

            return ret;
        }

        // 延时2个曝光时间，等待图像稳定
        sleep(exposure_ * 2);

        if (b_calibration == false)
            break;

        setAndNotifyCalibraterState(STATE_ASTRO_PLATE_SOLVING, i + 1);

        int ret = plateSolver_.start(eqCoord[success_times_], haCoord[success_times_]);
        if (ret == 0) {
            success_times_++;
            need_rotate_angle -= next_rotate_angle;
            next_rotate_angle = small_rotate_angle_;

            itips_observer_->updateCmdIdAndErrorCode(CMD_ASTRO_START_CALIBRATION, WS_OK);

            rgb_power_.setRgbEffect(RGB_EFFECT_WORKING_STATE);
        } else {
            need_rotate_angle += big_rotate_angle_ - next_rotate_angle;
            next_rotate_angle = big_rotate_angle_;

            itips_observer_->updateCmdIdAndErrorCode(CMD_ASTRO_START_CALIBRATION, CODE_ASTRO_PLATE_SOLVING_FAILED);
            wsCommonResponse(CMD_ASTRO_START_CALIBRATION, MODULE_ASTRO, CMD_TYPE_RESPONSE, -CODE_ASTRO_PLATE_SOLVING_FAILED);
        }

        if (success_times_ == need_success_times_)
            break;
    }

    // 成功need_success_times_次
    if (success_times_ == need_success_times_) {
        ret = calculateMyZenithCoord(haCoord, zenithCoord_);
        if (ret < 0)
            return ret;

        Ephemeris::haCoordToMyCoord(haCoord[2], zenithCoord_, lastMyCoord_);

        motor_yaw_.getAbsolutePositionInDegree(motor_resolution_level_, lastMechanicalCoord_.azi);
        motor_pitch_.getAbsolutePositionInDegree(motor_resolution_level_, lastMechanicalCoord_.alt);

        log_i("zenithCoord_.ha:%f, zenithCoord_.dec:%f", zenithCoord_.ha, zenithCoord_.dec);
        log_i("lastMyCoord_.azi:%f, lastMyCoord_.alt:%f", lastMyCoord_.azi, lastMyCoord_.alt);
        log_i("lastMechanicalCoord_.azi:%f, lastMechanicalCoord_.alt:%f", lastMechanicalCoord_.azi, lastMechanicalCoord_.alt);

        itips_observer_->updateCmdIdAndErrorCode(CMD_ASTRO_START_CALIBRATION, WS_OK);       
        wsCommonResponse(CMD_ASTRO_START_CALIBRATION, MODULE_ASTRO, CMD_TYPE_RESPONSE, WS_OK);

        setAndNotifyCalibraterState(STATE_STOPPED);
        setAndNotifyCalibraterState(STATE_IDLE);

        return 0;
    } else {
        // 转存到SD卡
        fs::path fitsFailedParentPath = failedPath / fitsFolderString;

        try {
            fs::copy(fitsTmpParentPath, fitsFailedParentPath);
        } catch (const std::filesystem::filesystem_error& e) {
            log_i("fs::copy:%s", e.what());
        }

        if (b_calibration == false) {
            if (itips_observer_->getErrorCode(CMD_ASTRO_START_CALIBRATION) == -1)
                itips_observer_->backToOldCmdIdAndErrorCode(CMD_ASTRO_START_CALIBRATION);
            wsCommonResponse(CMD_ASTRO_STOP_CALIBRATION, MODULE_ASTRO, CMD_TYPE_RESPONSE, WS_OK);

            setAndNotifyCalibraterState(STATE_STOPPED);
            setAndNotifyCalibraterState(STATE_IDLE);

            return 0;
        } else {
            itips_observer_->updateCmdIdAndErrorCode(CMD_ASTRO_START_CALIBRATION, CODE_ASTRO_CALIBRATION_PLATE_SOLVING_FAILED_TOO_MUCH);
            wsCommonResponse(CMD_ASTRO_START_CALIBRATION, MODULE_ASTRO, CMD_TYPE_RESPONSE, -CODE_ASTRO_CALIBRATION_FAILED);

            setAndNotifyCalibraterState(STATE_STOPPED);
            setAndNotifyCalibraterState(STATE_IDLE);

            return -CODE_ASTRO_CALIBRATION_FAILED;
        }
    }
}

int Calibrater::stop() {
    if (calibrater_state_.state == STATE_RUNNING || calibrater_state_.state == STATE_ASTRO_PLATE_SOLVING)
        setAndNotifyCalibraterState(STATE_STOPPING);
    else 
        setAndNotifyCalibraterState(calibrater_state_.state);

    if (calibrater_state_.state == STATE_IDLE)
        wsCommonResponse(CMD_ASTRO_STOP_CALIBRATION, MODULE_ASTRO, CMD_TYPE_RESPONSE, WS_OK);

    b_calibration = false;

    plateSolver_.stop();
    motor_yaw_.stop();
    motor_pitch_.stop();

    return 0;
}

int Calibrater::calculateMyZenithCoord(HourAngleCoordinates starHaCoord[3], HourAngleCoordinates& zenithSphericalCoord) const {
    // 球坐标(方位角, 高度角)，直角坐标(x, y, z)
    FLoat p1_S[2] = {starHaCoord[0].ha, starHaCoord[0].dec};
    FLoat p2_S[2] = {starHaCoord[1].ha, starHaCoord[1].dec};
    FLoat p3_S[2] = {starHaCoord[2].ha, starHaCoord[2].dec};
    FLoat result[2], p1[3], p2[3], p3[3], plane[4], zenithCartesianCoord[3];

    changeSphericalToCartesian(p1_S, p1);
    changeSphericalToCartesian(p2_S, p2);
    changeSphericalToCartesian(p3_S, p3);

    getPlaneEquation(p1, p2, p3, plane);

    FLoat equation[3][4] = {{2*(p1[0]-p2[0]), 2*(p1[1]-p2[1]), 2*(p1[2]-p2[2]), p1[0]*p1[0]+p1[1]*p1[1]+p1[2]*p1[2]-p2[0]*p2[0]-p2[1]*p2[1]-p2[2]*p2[2]}, 
                            {2*(p1[0]-p3[0]), 2*(p1[1]-p3[1]), 2*(p1[2]-p3[2]), p1[0]*p1[0]+p1[1]*p1[1]+p1[2]*p1[2]-p3[0]*p3[0]-p3[1]*p3[1]-p3[2]*p3[2]},
                            {plane[0],        plane[1],        plane[2],        plane[3]}};
    // 克拉默法则
    FLoat pass[3][3];
    transferAToB(pass, equation, 3);
    FLoat D = det3(pass); 
    if(D == 0) 
        return -1;// 方程无解，电机可能有误

    transferAToB(pass, equation, 0);
    FLoat D0 = det3(pass); 

    transferAToB(pass, equation, 1);
    FLoat D1 = det3(pass); 

    transferAToB(pass, equation, 2);
    FLoat D2 = det3(pass); 

    if(D0 == 0 && D1 == 0 && D2 == 0) 
        return -1;// 天顶半径为0，明显有误

    zenithCartesianCoord[0] = D0 / D;
    zenithCartesianCoord[1] = D1 / D;
    zenithCartesianCoord[2] = D2 / D;

    changeCartesianToSpherical(zenithCartesianCoord, result);
    zenithSphericalCoord.ha = result[0];
    zenithSphericalCoord.dec = result[1];

    return 0;
}

// 假设球半径为1
void Calibrater::changeSphericalToCartesian(FLoat p_S[2], FLoat p_D[3]) const {
    p_D[0] = COSD(p_S[0]) * COSD(p_S[1]);
    p_D[1] = SIND(p_S[0]) * COSD(p_S[1]);
    p_D[2] = SIND(p_S[1]);
}

// 斜面圆心不在球面上，转球坐标半径不为1
void Calibrater::changeCartesianToSpherical(FLoat p_D[3], FLoat p_S[2]) const {
    FLoat r = sqrt(p_D[0]*p_D[0] + p_D[1]*p_D[1] + p_D[2]*p_D[2]);
    p_S[1] = asin(p_D[2] / r);
    p_S[1] = RADIANS_TO_DEGREES(p_S[1]);

    p_S[0] = atan2(p_D[1], p_D[0]);
    p_S[0] = RADIANS_TO_DEGREES(p_S[0]);
    p_S[0] = LIMIT_DEGREES_TO_360(p_S[0]);
}

void Calibrater::getPlaneEquation(FLoat p1[3], FLoat p2[3], FLoat p3[3], FLoat result[4]) const {
    // A(x-x1)+B(y-y1)+C(z-z1)=0, Ax+By+Cz=D=Ax1+By1+Cz1
    result[0] = ((p2[1]-p1[1])*(p3[2]-p1[2])) - ((p2[2]-p1[2])*(p3[1]-p1[1]));  // A=(y2-y1)(z3-z1)-(z2-z1)(y3-y1)
    result[1] = ((p2[2]-p1[2])*(p3[0]-p1[0])) - ((p2[0]-p1[0])*(p3[2]-p1[2]));  // B=(z2-z1)(x3-x1)-(x2-x1)(z3-z1)
    result[2] = ((p2[0]-p1[0])*(p3[1]-p1[1])) - ((p2[1]-p1[1])*(p3[0]-p1[0]));  // C=(x2-x1)(y3-y1)-(y2-y1)(x3-x1)
    result[3] = result[0] * p1[0] + result[1] * p1[1] + result[2] * p1[2];      // D=Ax1+By1+Cz1
}

FLoat Calibrater::det3(FLoat a[3][3]) const {
    return a[0][0] * a[1][1] * a[2][2] + 
           a[0][1] * a[1][2] * a[2][0] + 
           a[0][2] * a[1][0] * a[2][1] - 
           a[0][2] * a[1][1] * a[2][0] - 
           a[0][1] * a[1][0] * a[2][2] - 
           a[0][0] * a[1][2] * a[2][1];
}

void Calibrater::transferAToB(FLoat pass[3][3], FLoat equation[3][4], int col) const {
    for(int j = 0; j < 3; j++) {
        if(j == col) 
            for(int i = 0; i < 3; i++) 
                pass[i][j] = equation[i][3];
        else 
            for(int i = 0; i < 3; i++) 
                pass[i][j] = equation[i][j]; 
    }
}

// 使用线程锁保护获取的状态和主动通知的状态之间的顺序，确保后面的状态不会先发送
void Calibrater::setAndNotifyCalibraterState(const StateNotify& state, int plate_solving_times) {
    std::unique_lock<std::mutex> lock(calibrater_state_mutex);

    calibrater_state_.state = state;
    if (calibrater_state_.state == STATE_ASTRO_PLATE_SOLVING)
        calibrater_state_.plate_solving_times = plate_solving_times;

    log_i("calibrater_state_:%d, plate_solving_times:%d", calibrater_state_.state.load(), calibrater_state_.plate_solving_times.load());

    wsResNotifyAstroCalibrationState(CMD_NOTIFY_STATE_ASTRO_CALIBRATION, MODULE_NOTIFY, CMD_TYPE_NOTIFICATION, calibrater_state_.state, calibrater_state_.plate_solving_times);
}

void Calibrater::notifyCalibraterState() {
    std::unique_lock<std::mutex> lock(calibrater_state_mutex);

    if (calibrater_state_.state != STATE_IDLE) {
        log_i("calibrater_state_:%d, plate_solving_times:%d", calibrater_state_.state.load(), calibrater_state_.plate_solving_times.load());
        wsResNotifyAstroCalibrationState(CMD_NOTIFY_STATE_ASTRO_CALIBRATION, MODULE_NOTIFY, CMD_TYPE_NOTIFICATION, calibrater_state_.state, calibrater_state_.plate_solving_times);
    }
}